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ABSTRACT 


The major problem addressed by this research is how to develop a motion control 
algorithm for local motion planning of an autonomous mobile robot. 
The approach taken was to clearly define the robot’s motion descriptions and to design 
a high-level, machine independent robot control language called MML (Model-based 
Mobile robot Language). 

The results are that the robot can implement smooth rotation, symmetric, and initial 
motion tracking on an given environment. Based on the motion control algorithm 
developed in this thesis, the robot with rigid body can be applicable in both local and global 
motion planning. 

The experimental results were successful. The algorithms were implemented first on 
a simulator, then on the autonomous mobile robot Yamabico-11. Given an initial and final 


configuration, the vehicle demonstrated the capability to safely achieve its goal. 
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I. INTRODUCTION 


One of the ultimate goals in robotics is to develop an autonomous robot - being capable of 
successfully completing a task given what to do and how to te it. Increasingly capable autono- 
mous robot platforms are being developed to handle a myriad of hazardous duty assignments. 
While manufacturing dominates the area of robotic applications, useful advances have been made 
in the areas of waste management, space exploration, undersea work, assistance for the disabled 
and medical surgery [Lato]. Several government-sponsored efforts are underway for building sys- 
_ tems for military applications such as fighting fires, handling ammunition, transporting material, 
conducting underwater search and inspection operations, and other dangerous tasks now per- 
formed by humans [Nava]. 

Many of the above tasks require motion of the robot in order to carry out any task. This 
problem is known as the motion planning problem. We divide the motion planning problem into 
two sub-problems: the global and local motion planning. The robot motion can be described as 
follows: a path of robot to carry out a task while moving from one configuration (p, 9, k) to 
another. In motion planning, not only is its position important, but the orientation and curvature of 
the robot are important as it follows the path. 

The purpose of the research is to investigate fundamental theories for navigation to con- 
struct autonomous mobile robot for military and industrial applications. In this thesis, we will dis- 
cuss several local motion planning problems inevitable for the robot to carry out any task. They 


are divided as follows: smooth rotation, symmetric, and initial motion planning. 














Il. BACK GROUND 


The focus of this thesis is on planning the motion of an autonomous mobile robot. The 
algorithms for motion planning to be taken by real robot are developed. Many simulations are 
done to verify the algorithms. 

A high-level motion specification language, Mobile-based Mobile-robot Language 
(MML-11), suitable to describe the solutions to the motion planning problem is used as the imple- 
mentation and verification of the algorithms. 


The world space Wfor the motion planning problem in this thesis is a two dimensional 


plane %* on which a global Cartesian coordinate system is defined. The obstacles are closed sub- 
sets of WW The free space, FREE(W), is the complement of the union of all obstacles in the world 
space W 

The vehicle in the thesis refers to a rigid body robot which has fixed shape - during the 
research we will use Yamabico-11 for experiments although the algorithms will be suitable for 
robots with any variable shape. 

A configuration g in this thesis defined as a combination of position, orientation, and cur- 
vature, (p, 8, k), where p indicates the position (x, y) in the global Cartesian coordinate system, 0 
is the orientation related to the x-axis of the global coordinate system [Loza], and k is the speci- 
fied curvature. The configuration defined in this thesis is normally used to describe the robot’s 
instantaneous status, either it is stationary or moving. This configuration is specially useful to 
specify a path. For instance, if we use g = ((x, y), 8, k) to specify a line, this line passes through 
the point at (x, y) and with orientation 8. When the curvature element k = 0, it is specifying a 


Straight line, otherwise it’s a circle. 





The motion of a vehicle is subject to nonholonomic and kinematic constraints, that is, the 
vehicle is able to perform both forward and backward motion but not sideways motion. The 


robot’s orientation and curvature at the path are continuous. 











Il. PROBLEM STATEMENTS 


The problem investigated in this thesis is intelligent behavior of autonomous mobile 
robots with emphasis on local motion planning and how to precisely and stably control the 
motions of the autonomous mobile robot Yamabico-1]. This problem is subdivided into the fol- 
lowing problems: 

1. What procedures should an autonomous mobile robot follow in order to rotate smoothly 
in a given environment? 

2. What procedures should an autonomous mobile robot follow to get to the configuration 
of not being on the center of the path? 

3. What procedures should an autonomous mobile robot follow to avoid the seiion of 


the walls and to get to the goal configuration? 




















IV. SMOOTH ROTATION 


A. MOTION DESCRIPTION 
We consider a situation where the vehicle at an arbitrary configuration gs = (ps, Qs, ks) 1s 
supposed to rotate its orientation from Qs to an arbitrary 8, without changing its position ps. (Fig- 


ure 1) 





Figure 1: Rotating of orientation without changing its position 


The motion of this vehicle can be described as follows: 
Start rotating the orientation towards the desired direction. 
Check the vehicle heading. 
Stop rotating when its heading equals to the goal orientation. 


As we can see, the motion does not involve any translation of the vehicle. So it is not nec- 








essary to use the steering function. Instead, the motion is based on relation between difference of 
angles and rotational speed of the vehicle. We need an algorithm which controls the rotational 
speed of the vehicle depending on the vehicle’s rotational angle. 
B. MOTION PLANNING 

l. Algorithm For Smooth Rotation 


The algorithm as Eq 4.1 is a tool for a mobile robot vehicle performing smooth rotation. 


at = —2kw-K (8,-8,) (Eq 4.1) 
where 2”: Vehicle’s rotational acceleration, 


dt 
w: Vehicle’s rotational speed, 
Q;: Vehicle’s initial orientation, 
8,: Vehicle’s goal orientation, 
k: Positive constant 
As we can see in Eq 4.1, the rotational speed is the only control variable of the algorithm. 
While rotating, the algorithm returns rotational acceleration depending on the current speed at 
each time, and hence, the speed of the vehicle also. 
2: Motion Planning With Algorithm 
In Eq 4.1, the positive constant k is an important factor of the algorithm. The bigger k we 
use, the rotational time is shorter, and the smaller k, it takes more time to rotate. Figure 2 shows 


the relation between speed and time for the vehicle to rotate x degrees with various k. 
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Figure 2: The graph for smooth rotation algorithm 


As shown in the figure 2, using bigger k, for example, k = 4 in the graph, gives the vehicle 
higher speed and shorter time to complete its mission But in some cases, it can be dangerous for 
the vehicle if the speed exceeds the vehicle’s speed limit. There can be no such high speed in 
using smaller k, but it has two defects: one being too much time to do its job, and the other being 


the lower speed duration time is too long. 








C. EXPERIMANTAL RESULT 


Considering rotational time, we used k = 4 curve, but for the safety of the vehicle, we limit 


the speed with w = 3. Figure 3 shows the result. 


SUMMA M>ZO™S>PHOR 





TIME (t) 


Figure 3: The graph for redefined algorithm 


As shown by the result, applying the speed limit makes the algorithm more useful. 
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V. SYMMETRIC MOTION PLANNING 


A. | MOTION DESCRIPTION 

We consider a situation where the vehicle is supposed to track a path, and to get to the goal 
position which may not be on the center of the path. A vehicle is represented by a point ps = (xs, 
ys), its orientation Os, and curvature ks, and the goal position is represented by pz = (Xe, ye), and its 


orientation Oz, and its curvature k,;. Figure 4 shows the situation. 





Figure 4: Symmetric motion 


The motion of this vehicle can be divided into two parts; one being the motion on the path 
A, and the other being the motion on the curve B. The first part of the motion is easily done by the 
steering function, but the motion on the curve B is difficult to make it using the transformation. 
For the safety of vehicle, it is important to keep moving on the center of path as long as possible. 
B. MOTION PLANNING WITH STEERING FUNCTION 

1. Line Tracking 


Let’s assume a directed straight line L represented by a point (a, 5) on it, and its orienta- 
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tion a. A vehicle away from the line is supposed to track the line. The vehicle is represented by p 


= (x, y), and its orientation 9. (Figure 5) 





Figure 5: Line tracking 


The steering function for the vehicle supposed to track the line L is [Kana] 


= = _(ak+b(®—«a) +c(— (x—a) sina + (y—b) cosa) ) (Eq 5.1) 


Using this steering function, a vehicle tracks a dotted line as shown in Fig. 5.2. How the 
steering function accomplishes a line tracking is fully described in [Kana], [Macp], and [Kova]. 

Z Symmetric Motion 

As seen in Section A in this Chapter, the motion B is a reverse action of line tracking. 
Let’s assume a virtual vehicle which tracks a line from goal position. There will be a path kept by 
the virtual vehicle. With the path in the reverse direction, the real vehicle can move toward the 
goal position. 

a. Virtual Vehicle Simulation 


We let 
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Start Configuration: gs = ((Xs, ys), gs, Ks), 
Goal Configuration: gz = ((Xz, ys), ds, ke) 
Let’s assume there is a virtual vehicle at the goal position with reverse direction. 


The virtual vehicle is supposed to track a directed straight line L represented by the start configu- 





ration gs. 








Figure 6: Virtual vehicle tracking 


We can get 
ka= -ks, 
Oa = Qs + Tl, 


Ad = -(x - Xs)sin@a + (y - ys)cos®u, 
because the virtual vehicle uses the gs as its goal configuration. 


The virtual vehicle steering function 1s 


= = —(ak+b(@-0,) +c(- (x—x,) sin@, + (y—y,) cos®,)) (Eq 5.2) 


Using this steering function, the virtual vehicle tracks the line L, and finally gets to 
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the start configuration. Configurations generated during the virtual vehicle move are stored in 
array to be used by real vehicle later. When storing configurations, 8 is changed to 0 + 1, and 
kappa is changed its sign(+/-). 

b. Real Vehicle Moving while Simulation 

While the virtual vehicle simulates a line tracking, the real vehicle moves forward 
also. There will be some point between start configuration qs and goal configuration gz where the 
real vehicle meets the virtual one. The point has an important role in symmetric motion planning, 
because the virtual vehicle finishes its line tracking simulation, and the real vehicle starts tracking 


a simulated path at the point. We need to know how to get the point. 





Figure 7: Real vehicle moving 


As shown in Figure 7, we can get the distance between the real vehicle and the 
virtual one by computing the local coordinate x* - detailed explanation for the local coordinate 
will be discussed in later subsection. When the distance equals to 0, it is the time the real vehicle 


starts tracking a simulated path generated by virtual vehicle. 
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c. Local Coordinate 

We take simulated configurations by the virtual vehicle sparser than those gener- 
ated by the real vehicle. In addition to the data, the ‘al vehicle uses the steering function locally 
between the configurations. 

Let’s assume 

Vehicle configuration: gq = ((x, y), 8, k), 

Target configuration: gc = ((Xc, yc), Oc, Ke) 
To use the steering function, we need to compute a local coordinate g* = ((x*, y*), O*, k*) 


between vehicle and target configuration. (Figure 8) 





Figure 8: Local coordinate 


The step how to compute local coordinate g* is as follows: 
ge = G0 q*, 
q’ 0 Ge = (q’ 0g) 0 q* 

As a result, we get 


galiog: (Eq 5.3) 
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With the Eq 5.3, we can compute local eponiinates as follows [Kana95] 
x* = (x - x-)cosO + (y - yc)sinO (Eq 5.4) 
y* = (x - x-)sin8 + (y- yc)cos8 (Eq 5.5) 
d. Real Vehicle Tracking 
As shown in Fig. 5.4, the real vehicle uses a configuration of the simulated path 
one by one as its target. The step how to track the simulated path is as follows: 
(1) Compute local coordinate between vehicle and target configuration 
(2) Check x* 
(2.1) If x* <=0 
(2.1.1) If the target is goal configuration 
(2.1.1.1) Stop vehicle 
(2.1.2) If the target is not goal configuration 
(2.1.2.1) Change the target with a new configuration of the simulated path 
(2.2) If x* > 0 
(2.3) Move the vehicle using steering function 


The real vehcle uses the steering function as follows: 


= —(aAk +bA® +cAd) (Eq 5.6) 


where Ak=k-kc, 
AO = 8 - 8c, 


Ad = y* 








Cc: EXPERIMENTAL RESULT 
Figure 9 shows the result in which the real vehicle uses the same smoothness as the virtual 


vehicle does. The vehicle’s initial configuration is (0, 0, 0). The vehicle’s goal position is (400, 





60) and its orientation is 0 degrees. As a result, the real vehicle gets to the configuration (400, 63, 


| 0). 


"Real Path’ —— 





Figure 9: The result with the same o as the virtual vehicle does 


17 








Figure 10 shows the result in which the real vehicle uses one half smoothness of the vir- 
tual vehicle. The vehicle’s initial configuration is (0, 0, 0). The vehicle’s goal position is (400, 60) 


and its orientation is 0 degrees. As a result, the real vehicle gets to the configuration (400, 60.5, 0). 


*Real_Path’ —— 





Figure 10: The result with a half of virtual vehicle o 
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Figure 11 shows the result in which the real vehicle uses one quarter smoothness of the 


virtual vehicle. The vehicle’s initial configuration is (0, 0, 0). The vehicle’s goal position is (400, 


60) and its orientation is 0 degrees. As a result, the real vehicle successfully gets to the configura- 


tion (400, 60, 0). 





“Real Path’ — 


Figure 11: The result with a quarter of virtual vehicle o 
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VI. INITIAL MOTION PLANNING 


A. MOTION DESCRIPTION 
We consider a situation where the vehicle at an arbitrary configuration gs = (ps, 8s, ks) is 
located on the world which is consisted of walls. There is one border which has a configuration 


called Exit configuration ge = (pe, Qe, ke). Figure 12 illustrates this situation. 


mee 
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Figure 12: The case where simulated path collides 


The world where vehicles are located is a part of k-regions [Kana95][Kova]. A vehicle 
located in the world has a mission to find a path which can converge to the exit configuration ge 
without colliding the wall. The mission is a kind of line tracking, that is, the vehicle with a config- 


uration gs is supposed to track a straight line gref represented by pe and its orientation Qe using 
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steering function. As shown in the figure, since the vehicle at configuration gs: is too close to the 
wall, the vehicle will collide the wall. For the safety of the vehicle, this behavior can not be 
accepted. The other vehicle at configuration qs2 is far away from the wall, but the vehicle fails to 
pass the exit configuration ge. The exit configuration qe is the start configuration of next region. 
It’s important for the vehicle to get to the exact configuration. The mission can be divided into 
two parts: one being the mission to find a path to avoid colliding of walls, and the other being the 
mission to find a path to get to the exit configuration ge. 

B. MOTION PLANNING WITH STEERING FUNCTION 


The steering function (Eq 6.1) is used for a vehicle to perform smooth line tracking. 


- = —(AAk +BA0+CAd) (Eq 6.1) 


In Eq 6.1, A, B, and C are positive constants which are related to the smoothness of vehi- 


cle’s motion [Kana]. They are determined by the smoothness o as follows: 


k=1/o (Eq 6.2) 
A=3*k (Eq 6.3) 
B=3*k*k (Eq 6.4) 
C=3*k*k*k (Eq 6.5) 


From the above equations, we know the smoothness 6 determines the sharpness of the 
tracking trajectory. We can see that the smaller the value of smoothness is, the sharper the trajec- 
tory will be, and the larger the smoothness is, the longer the vehicle travels [Kana]. 

While we know tracking trajectory depends on smoothness, it is important to know that a 
lower limit of smoothness exists in using the steering function to track a line. The lower limit of 


smoothness is as follows [Chuang]: 


6 = 0.095 * Adinit (Eq 6.6) 
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In Eq 6.6, Adinit is the closest distance from the initial configuration to the reference line. If 
a smaller smoothness 6 than the lower limit is applied, the tracking trajectory never converge to 
the reference line. 

In addition to the lower limit of smoothness, we have to consider there will be undesirable 
tracking trajectories to converge to the reference line with some arbitrary smoothness. Although 
they converge to the reference line, the trajectory travels backward. The minimum desirable 
smoothness for parallel line tracking is as follows [Chuang]: 

6 = 0.18 * Adinit (Eq 6.7) 

1, Motion Planning to Avoid The Collision 

A line tracking motion starts from a configuration gs = (ps, 9s, ks). The goal of the line 
tracking is the line called reference line gre, specified by a configuration ge = (pe, Oe, ke). Since the 


line is a straight line, it has a constant curvature ke = 0. Figure 13 shows the case. 





Figure 13: The case to avoid collision 
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The path of line tracking in the figure refers to a straight line gre. But we can make the Eq 
6.1 simpler using gre as a reference line instead of using gre. We can use a bigger smoothness to 
converge to the reference line, because the Ad is 0. 


- = —(AAk + BA) (Eq 6.8) 


The step to avoid collision is as follows: 
(1) Start line tracking simulation with default smoothness 
(2) Check if path collides the wall 
- Checking collision will be discussed later in this chapter 
(2.1) If the path collides the wall 
(2.1.1) Change the smoothness to a smaller one 
(2.1.1.1) Check if the smoothness is smaller than 0.095 * Adinit 
(2.1.1.1.1) Quit the simulation 
(2.1.1.1.2) No path to avoid collision exists 
(2.1.2) Start line tracking simulation again with new smoothness 
(2.2) If there is no collision on the path 
(2.2.1) Check if the path is parallel to the exit configuration qe 
(2.2.2) If the path is parallel to the exit configuration, stop the simulation. 
(3) Return the smoothness 6 
2; Motion Planning to Converge to The Reference Line 
A line tracking motion starting from a configuration qs = (ps, 8s, ks) reached to a configura- 
tion Gn = (pn, On, kn) which is parallel to the exit configuration ge. From the configuration qn = (pn, 
8., kn), another line tracking motion starts to converge to the reference line qr specified by ge = 


(De, Qe, ke). Figure 14 shows this case. 








Figure 14: The case to converge to the reference line 


The path of line tracking in the figure refers to a straight line dre. A line tracking starts 
with default smoothness © again. The reason not using smoothness which we got in Section 1 is 
that the simulated path could be sharper to avoid the collision, that is, the smoothness would be 
smaller than 0.18 * Adini:. A line tracking path with the smoothness will be undesirable to con- 
verge to the reference line - the detailed explanation is discussed in Section 1 in this Chapter. 

The step to converge to the reference line is as follows: 

(1) Start line tracking simulation with default smoothness 
(2) Check if x* equals to 0, or smaller than 0 
(1.1) Check if y* equals to 0 
(1.1.1) Return the smoothness o 


(1.1.2) Quit the simulation 
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(1.2) If y* does not equal to 0 
(1.2.1) Change the smoothness to a smaller one 
(1.2.2) Check if the smoothness is smaller than 0.18 * Adinit 
(1.2.2.1) Quit the simulation 
(1.2.2.2) No path exists 
(3) Start line tracking simulation again with new smoothness 
3. Motion Planning to Determine The Direction of Path 
A line tracking motion starting from a configuration qs = (ps, 9s, ks) will get to the goal 
configuration ge = (pe, 8c, ke) on one desirable path. But actually, regarding to the heading of the 


vehicle, there can exist more desirable path. Figure 15 shows this case. 





Figure 15: The case of various direction of paths 


The direction of the path is related not only to the heading of the vehicle 0, but to the 
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orientation of the Exit configuration @c. 
The step to determine the direction of the path is as follows: 
(A) ID(Os — Be)| <= 1/2 
- ® means normalization 
(1) If (6s - 8e) >a /2) ThenO@=O8s-27 
(2) Else if ((@s - 8e) <x /2) ThenO@=Os-27 
(B) ID(Os — Be)! > x / 2 
(1) y* >=0 
(a) If ((Os - Be) > 0) Then 8 = 9s - 27 
(b) Else if ((Os - 8c) < -2 1) Then 8=9s-27 
(2) y* <0 
(a) If ((Os - 8c) > 2 2) Then8=O0s-27 
(b) Else if ((@s - 9c) < 0) Then 9 =0s+27 
4. Motion Planning to Check Collision 
In a motion planning simulation from a configuration gs = (ps, Qs, ks) to find a desirable 
path to get to the exit configuration ge = (pe, Oe, ke), it is important to check if the path collides the 
wall. To test it, we need to know how the world 1s described. 
a. Inverted World 
We consider a two-dimensional world W with holes. A hole is an obstacle for a 
vehicle. A free space Free(W) is the complement of the union of all holes. There might be a hole 
among them which completely surrounds the free space. The hole like this is said to be inverted. 


Every free space is a connected subset of W. Figure 16 shows this world. 
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Figure 16: Inverted World 


b. View Angle 


With a polygon B and a point p which is not on its boundary, the view angle yi(p) of 


the ith edge v, @(v) at p is defined as follows (Figure 17) : 


¥;(p) = BCE (p, 9(v,;)) —P @, V;)) (Eq 6.9) 





Figure 17: View Angle at point p 
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The sum of the n view angles in B is defined as 


ra(p) = 27; (P) (Eq 6.10) 
ps) 








In the inverted world, if s(p) = -27, the point p is in Free(W), otherwise, 


| it’s not in Free(W) [Kana]. 





c. Collision Test of Vehicle 

So far, the vehicle in the motion planning has been dealt as if one point. But it is 
not enough to test if the vehicle collides the walls. In real world, the vehicle has a rigid body with 
size such as width and length. There might be a situation while one corner of the vehicle is in the 
free space, the other corner could collide the wall. This behavior can not be accepted. We need to 


know how to test collision of each corner of the vehicle at every step of the motion planning. 


wall 





Figure 18: Collision test of vehicle 
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As shown in the Figure 18, we know two important informations such that the con- 


figuration of the center of the vehicle g which is on the motion planning path, and the local coor- 


dinates of vehcle’s corners such as cl, c2, c3, c4, c5. With the information, we can get each 


corner’s configuration as follows: 


ner. 


qgl=qocl, 
q2 =q 0C2, 
q3 =q 003, 
q4=qoc4, 
qs =qocs 


Instead of testing center of the vehicle, the collision test will be doing at each cor- 
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Cc. EXPERIMENTAL RESULT 
Figure 19 shows the result in which the vehicle located at a configuration gs = ((50.0, 


100.0), -37/4, 0.0) finds a path to converge to the exit configuration ge = ((175.0, 679.704), 1/2, 





0.0) avoiding a collision of the walls. The default smoothness © is 80. The vehicle used in the 


experiment has the size: width = 50cm, length = 60cm. 








Figure 19: The result the vehicle finds a path 
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APPENDIX 


A. C PROGRAM FOR SMOOTH ROTATION 


[RRR AAA RAR RAK KR RAR KKK RAKE EAA KA HARK EAR A ARH BR HH HH KEE 


Function : rotateRule() 

Purpose : updates the commanded velocity to rotate the robot 
Parameters: VELOCITY actual, commanded 

Returns : The required linear velocity, rotational velocity 
Called by : 

Calls: limitQ) 


Hh 2 eK 2 6 246 2A ee CH 2 a HE He EE A He A A HE A A Ae 2 Ae a 2 2 A A AK RE HE AE HE / 


static VELOCITY 
rotateRule( VELOCITY actual, VELOCITY commanded) 
{ 

double k = 0.5; 

static double goalTheta; 

static int noGoalTheta = TRUE; 

static double rotVel = 0.0; 

double rotAcc = 0.0; 


if (noGoalTheta) { 
/* set the goal of rotation */ 
goalTheta = vehicle. Theta + currentPath.config. Theta; 
noGoalTheta = FALSE; 

} 


commanded.Linear = 0.0; 


if (fabs(vehicle.Theta - goalTheta) >= 0.005) { 
rotAcc = -2 *k * rotVel-k*k* 
(vehicle.Theta - goalTheta); 
rotAcc = limit(rotAcc, DEFAULT_ROT_ACC); 
rotVel = rotVel + rotAcc * MOTION_CONTROL_ CYCLE; 
rotVel = limit(rotVel, DEFAULT_GOAL_VEL_ROT); 
commanded.Rotational = rotVel; 


else { /* the rotation 1s completed */ 
currentPath.pathType.mode = STOPMODE; 
commanded.Rotational = 0.0; 
noGoalTheta = TRUE; 
rotVel = 0.0; 

} 


return commanded; 


33 








[28 RRA AA RR RA HH HHH HRI AR I HB RRR AAA HR RH IE I ER RR IEE EE 


Function : limit 

Purpose : This function is used by rotate function 
to limit rotational speed of the robot 

Parameters: double y, | 

Returns : double y,1 

Called by : rotateRuleQ 

Calls : None 


2 oe 2 2 2 2 ee 2 2 ee a a 2 ae 2 Ke a a a a AK HA AK HH A AK KR RK AE EK RK / 


double 
limit(double y, double 1) 
{ 


if (y>Dy=] 
else if (y < -l) y =-l; 


return y; 
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B. C PROGRAM FOR SYMMETRIC MOTION PLANNING 


[7 8 AR 2 He a aR eo HH A He HR HK HH HK HH HK HA HH He HB AK He AR HH HH A EK KK A EE 


Module Name: Header file 
Purpose: define variables 
Parameters: 

Returns: 

Called by: 

Calls: 


2 2s 9 2 2 2 2 2k 2 a oA oe a I A 2 ae a a A A A a A A KR AK OK RK KK KK EE ER KKK KR | 


#include <iostream.h> 
#include <math.h> 


#define PI 3.1415926 


typedef struct { 
double X; 
double Y; 

}POINT; 


typedef struct { 
POINT Posit; 
double Theta; 
double Kappa; 

}CONFIGURATION; 


CONFIGURATION symConfig[100000}; 
/* smoothness for virtual and real robot */ 
double sigma = 20; 
double sigma2 = 1; 


/* different delta s is used in calculated VirtualRobotPath() */ 
double deltaS = 0.1; 
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/* constant for steering function used in simulation */ 
double k = 1.0 / sigma; 

double a = 3 * k; 

double b = 3 * k * k; 

double c=k * k * k; 


/* constant for steering function used in real robot */ 
double kk = 1.0 / sigma?2; 

double aa = 3 * kk; 

double bb = 3 * kk * kk; 

double cc = kk * kk * kk; 


/* define functions used in the program */ 

CONFIGURATION moveRealRobot(CONFIGURATION q); 

CONFIGURATION calculate VirtualRobotPath(CONFIGURATION gq, CONFIGURATION L); 
CONFIGURATION moveRobotOnSimulatedPath(CONFIGURATION gq, int index); 

double computeS ymmetricPathKappa(CONFIGURATION gq, int 1); 

double getKappa(CONFIGURATION gq, CONFIGURATION L); 


/* library functions */ 

CONFIGURATION computeQstar(CONFIGURATION ql, CONFIGURATION q2); 
CONFIGURATION composite(CONFIGURATION ql, CONFIGURATION q2); 
CONFIGURATION circ(double deltaS, double theta); 

double norm(double angle); 
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[7 RA RH A HE A HH A KR RH A HH A RA IE RIE I HE HE HE 


Module Name: main function 
Purpose: for symmetric motion 
Parameters: 

Returns: 

Called by: 

Calls: 


2 He he fee fe Fe 2 ae 2 Ee eK He Hee 2 eB He ee a A A A He A A ee a I HH HK OK HR HK IK AK He AH A / 


main() 


CONFIGURATION gq, L, qstar; 
static VirtualRobotMoving = 1; 
static RealRobotMoving = 0; 
Static int initialSetting = 1; 
Static int index = Q; 

Static int 1, LargestIndex = 0; 


/* robot's current configuration */ 
q.Posit.X = vehicle.Posit.X, q.Posit. Y = vehicle.Posit.Y, 
q. Theta = vehicle. Theta, q.Kappa = vehicle. Kappa; 


if (initialSetting) { 
/* robot's goal configuration */ 
L.Posit.X = path.Posit.X, L.Posit. Y = path.Posit.Y, 
L.Theta = path. Theta + PI, L.Kappa = -path.Kappa; 
initialSetting = 0; 


} 


/* store the goal configuration to array indexed zero */ 
symConfig[index].Posit.X = L.Posit.X; 
symConfig[index].Posit.Y = L.Posit. Y; 
symConfig[index].Theta = L. Theta; 
symConfig[index].Kappa = L.Kappa; 


/* Ist part 
move real robot while simulating virtual robot */ 


while (VirtualRobotMoving) { 
q = moveRealRobot(q); /* move real robot */ 
/* compute new configuration path on which virtual robot moves, 
so the virtual robot L tracks the line defined by 


configuration q */ 
L = calculate VirtualRobotPath(L, q); 
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/* store new simulated configuration to array */ 
index++; 

symConfig[index].Posit.X = L.Posit.X; 
symConfig[index].Posit. Y = L.Posit. Y; 
symConfig[index].Theta = L.Theta - PI; 
symConfig[index].Kappa = -L.Kappa; 


/* test xstar if the real robot passes some point 
where the real robot meets the virtual robot */ 


qstar = computeQstar(symConfig[index], q); 
if (qstar.Posit.X <= 0.0) { 
VirtualRobotMoving = 0; 
RealRobotMoving = 1; 
} 
} 


/* 2nd part 
make real robot track the simulated path until the index is 0 */ 


while(RealRobotMoving) { 
if (index >= 0) { 


/* test xstar if the robot passes local target configuration 
on simulated path */ 


qstar = computeQstar(symConfig[index], q); 


if (qstar.Posit.X >= 0.0) { 
q = moveRobotOnSimulatedPath(q, index); 
} 
else index--; 
} 
else { 
LEDon(4); 
currentPath.pathType.mode = STOPMODE; 
commanded.Linear = 0.0; 
commanded.Rotational = 0.0; 
initialSetting = 1; 
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Module Name: moveRealRobot function 
Purpose: move real robot step forward 
Parameters: q | 

Returns: q 

Called by: main 


Calls: circ), composite() 
He ae 2k 2 fe 2 ae 2k eof 2k he 2h ef 2 2 2K 9s 2 2 ee oe he 2 2 he 2 2 eh ee 2 2 2k a A HK HK AK HK A EH HE HB HR HE / 


CONFIGURATION 
moveRealRobot(CONFIGURATION q) 
{ 

CONFIGURATION deltaQ; 

double dtheta; 





dtheta = 0.0; 

deltaQ = circ(deltaS, dtheta); 
q = composite(q, deltaQ); 
q.kappa = 0.0; 


return q; 
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Module Name: calculate VirtualRobotPath function 
Purpose: track the virtual robot to the reference line 
Parameters: q, L 

Returns: q 

Called by: main 

Calls: getKappa(Q), circ(), composite() 


ake 3k af a ak 2c 2K 2 ak 2 2 2k fe fe oe 2 ee ee 2 2g oe fs a AC 9K KK 2 2K a 2k 2 oe AC A A A eR KK I KH KK EK KE | 


CONFIGURATION 
calculate VirtualRobotPath(CONFIGURATION gq, CONFIGURATION L) 
{ 

CONFIGURATION deltaQ; 

double dkappa; 

double theta; 

double DifferentDeltaS = 0.2; 


L.theta = L.theta + PI; 

dkappa = q.Kappa + getKappa(q, L) * DifferentDeltaS; 
theta = DifferentDeltaS * dkappa; 

deltaQ = circ(DifferentDeltaS, theta); 

q = composite(q, deltaQ); 

q.Kappa = dkappa; 


return q; 
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Module Name: moveRobotOnsimulatedPath function 
Purpose: track the real robot on simulated path 
Parameters: q, 1 

Returns: q 

Called by: main 


Calls: computeS ymmetricPathKappa(), circ(0, compositeQ 
#2 He he 2h 2k he 2 2k 2 2 2s ee 2 2 he he 2 2 ee 2 9 A 2 oe 2K 2 HK 2 He A A HK HK KH HK RI HE ACK RAK KKK | 


CONFIGURATION 
moveRobotOnSimulatedPath(CONFIGURATION gq, int index) 


{ 
CONFIGURATION deltaQ; 
double dkappa, dtheta; 


dkappa = g.Kappa + computeSymmetricPathKappa(q, index) * deltaS; 
dtheta = deltaS * dkappa; 

deltaQ = circ(deltaS, dtheta); 

q = composite(q, deltaQ); 

q.kappa = dkappa; 


return q; 
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Module Name: computeSymmetricPathKappa function 
Purpose: compute kappa for real robot on simulated path 
Parameters: q, 1 

Returns: kappa 

Called by: moveRobotOnSimulatedPath() 


Calls: computeQstar() 
ok 2 He 2K ae 2 a 2 ae A A Ae a KC aR A A HK HK A HK HK HAA He a He HH HE A AK A A A OR HK HK KK HK HH HK | 


double 
computeSymmetricPathKappa(CONFIGURATION gq, int 1) 


{ 
CONFIGURATION gqstar; 


double deltaKappa, deltaTheta, deltaD; 

qstar = computeQstar(q, symConfig[1]); 
deltaKappa = q.Kappa - symConfig[i].Kappa; 
deltaTheta = q.Theta - symConfig[1]. Theta; 
deltaD = qstar.Posit. Y; 


return -(aa * deltaKappa + bb * norm(deltaTheta) + cc * deltaD); 
} 
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Module Name: getKappa function 
Purpose: compute kappa for virtual robot 
Parameters: gq, L 

Returns: kappa 

Called by: main 

Calls: 


7k 2s 26 262 ee 2k 2 ae 24 2 2 2 ee 2 ee 2 2 ee He BH A He 2 he He 2 ee 2 He 2 He A A A A 2 HR A A I I KEK KK EK | 


double 
getKappa(CONFIGURATION g, CONFIGURATION L) 


{ 
double deltaD; 


deltaD = -(q.Posit.X - L.Posit.X) * sin(L.Theta) + 
(q.Posit. Y - L.Posit. Y) * cos(L.Theta); 


return -(a * q.Kappa + b * (q.Theta - L.Theta) + c * deltaD); 
} 
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C; C PROGRAM FOR INITIAL MOTION PLANNING 


[HAAR A AAA AAR HRA HAR RR HR RH AAA RAK ARH KHER HEE EE EERE 


Module Name: Header file 
Purpose: define variables 
Parameters: 

Returns: 

Called by: 

Calls: 


she 2 of 2 2 2A 2 2 HH A I RK KK KA AK KER ER KKK KKK KKK EK KE / 


#include <iostream.h> 
#include <math.h> 


#define PI 3.1415926 
#define initialSigma 20 
#define ABS(x) ((x < 0.0) ? -x : x) 


typedef struct { 
double X; 
double Y; 

}POINT; 


typedef struct { 
POINT Posit; 
double Theta; 
double Kappa; 

}CONFIGURATION; 


/* define robot size */ 

POINT corner[4]; 

CONFIGURATION robotCorner][4}]; 
CONFIGURATION foreRunner[ 1000000}; 


double sigma, k, a, b, c; 








a 


/* define functions used in program */ 

CONFIGURATION determineDirectionOfMotion(qs, ge); 

void getConstantsforForerunner(double sigma); 

CONFIGURATION runForerunner(CONFIGURATION g, CONFIGURATION ge); 
double getKappa(CONFIGURATION g, CONFIGURATION L); 

int checklfcollide( CONFIGURATION q); 

double calculateAngle(CONFIGURATION gq, int i, int j); 

int checkIfQparallelQE(CONFIGURATION qi, CONFIGURATION q2); 

int checkIfQequalsQE(CONFIGURATION gq); 








/* library functions */ 
CONFIGURATION computeQstar(CONFIGURATION qi, CONFIGURATION q2); 
~CONFIGURATION composite(CONFIGURATION q1, CONFIGURATION q2); 
CONFIGURATION circ(double deltaS, double theta); 
double norm(double angle); 
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Module Name: main function 
Purpose: for initial motion 
Parameters: - - 
Returns: 

Called by: 

Calls: 


nf of ok a 2 ae ak 2k ae ae ak 2 2k 2K 2 he 2 ae ae 2 2 he ee ee ae 2K oe a IK 2 ae ee A KH A ae a Ke AK HK IK AK A KE EE 


main() 
{ 
CONFIGURATION gq, qs, ge, qd, qstar; 
int foreRunnerMoving = 1; 
int index = 0, midPoint; 
int 1 
double firstSigma, dinit; 
sigma = initialSigma; 
FILE *stream; 


/* for printing simulated path */ 
stream = fopen("data", "wt"); 
if(stream == NULL) fputs("File not found!\n", stderr), exit(1); 


/* robot's size */ 

robotCorner[0].Posit.X = -25.0, robotCormer[0].Posit.-Y = 30.0; 
robotCorner[1].Posit.X = 25.0, robotCorner[1].Posit.Y = 30.0; 
robotCorner[2].Posit.X = 25.0, robotCorner[2].Posit.Y = -30.0; 
robotCorner[3].Posit.X = -25.0, robotCorner[3].Posit.Y = -30.0; 


/* robot's initial and goal configuration */ 
qs.Posit.X = 50.0, qs.Posit.Y = 100.0, qs.Theta = -3 * PI / 4.0, qs.Kappa = 0.0; 
qe.Posit.X = 200.0, ge.Posit. Y = 700.00, qe.Theta = PI / 2.0, ge. Kappa = 0.0; 


/* region where robot starts moving */ 
corner[0].X = 0.0, corner[0].Y = 0.0; 
corner[1].X = 0.0, corner[1].Y = 700.00; 
comer[2}.X = 200.00, corner[2].Y = 700.00; 
comer[3].X = 200.00, corner[3].Y = 0.0; 


qstar = computeQstar(qs, ge); 
dinit = qstar.Posit.Y; 
qstar.Posit.X = 0.0; 


qd = composite(qge, qstar); 
qd.Theta = qe. Theta; 


46 








/* store the initial configuration to array indexed zero */ 
foreRunner[index].Posit.X = qs.Posit.X; 
foreRunner[index].Posit. Y = qs.Posit.Y; 
foreRunner[index].Theta = qs.Theta; 
foreRunner[index].Kappa = qs.Kappa; 


/* set the current configuration to start configuration */ 
q.Posit.X = qs.Posit.X; 

q.Posit. Y = qs.Posit.Y; 

q. Theta = qs. Theta; 

q.Kappa = qs.Kappa; 


/* set constants with sigma */ 
getConstantsforForerunner(sigma); 


while (foreRunnerMoving) { 
q = runForerunner(q, qd); 
index++; 


/* store the new configuration to array */ 
foreRunner[index].Posit.X = q.Posit.X; 
foreRunner[index].Posit.Y = q.Posit.Y; 
foreRunner[index].Theta = q. Theta; 
foreRunner[index].Kappa = q.Kappa; 


if (checkIfcollide(q) == 1) { 

sigma = sigma / 2.0; 

if (sigma < 0.095 * dinit) { 
printf(""Sorry, no possible path to avoid collision!\n"); 
exit(Q); 

} 

index = 0; 

q.Posit.X = qs.Posit.X; 

q.Posit. Y = qs.Posit. Y; 

q. Theta = qs. Theta; 

q.Kappa = qs.Kappa; 


getConstantsforForerunner(sigma); 


} 


if (checkIfQparallelQE(q, qd)) { 
firstSigma = sigma; 
midPoint = index; 
foreRunnerMoving = 0; 


} 
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} 


} /* end of while loop */ 


foreRunnerMoving = 1; 
sigma = initialSigma; 


while (foreRunnerMoving) { 
q =runForerunner(q, ge); 
index++; 


/* store the new configuration to array */ 
foreRunner[index].Posit.X = q.Posit.X; 
foreRunner[index].Posit.Y = q.Posit.Y; 
foreRunner[index].Theta = q. Theta; 
foreRunner[index].Kappa = q.Kappa; 


qstar = computeQstar(qe, q); 


if (qstar.Posit.X <= 0.0) { 

if (checkIfQequalsQE(qstar)) 
foreRunnerMoving = 0; 

else { 
sigma = sigma / 2.0; 
index = midPoint; 
q.Posit.X = foreRunner[index].Posit.X; 
q.Posit.Y = foreRunner[index].Posit. Y; 
q. Theta = foreRunner[index]. Theta; 
q.Kappa = foreRunner[index}. Kappa; 


getConstantsforForerunner(sigma); 
} 


} 
} /* end of while loop */ 
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Module Name: determineDirectionOfMotion function 
Purpose: determine the direction of robot moving 
Parameters: qs, ge 

Returns: qs 

Called by: main 


Calls: norm 
dela oa eS a a aC CI AGRIC IIR AICI IIR ASIII ICR I ICR RICK ACR II / 


CONFIGURATION 
determineDirectionOfMotion(qs, qe) 
{ 
if (ABS(norm(qs.Theta - ge.Theta)) <= PI / 2.0) { 
if (qs. Theta - qe. Theta > PI / 2.0) qs. Theta = qs.Theta - 2 * PI; 
else if (qs. Theta - ge.Theta < - PI / 2.0) qs. Theta = qs.Theta + 2 * PI; 


} 
else if (ABS(norm(qs.Theta - ge.Theta)) > PI / 2.0) { 
if (qstar.Posit.Y >= 0.0) { 
if (qs.Theta - ge.Theta > 0.0) qs. Theta = qs.Theta - 2 * PI; 
else if (qs. Theta - qe.Theta < - 2 * PI) qs. Theta = qs.Theta + 2 * PI; 


} 
else if (qstar.Posit.Y < 0.0) { 
if (qs. Theta - qe.Theta < 0.0) qs. Theta = qs. Theta + 2 * PI; 
else if (qs.Theta - ge.Theta > 2 * PI) qs.Theta = qs.Theta - 2 * PI; 
} 
} 


return qs; 


j 
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Module Name: getConstantsforForerunner function 
Purpose: compute constants a, b, c with new smoothness 
Parameters: sigma 

Returns: 

Called by: main 

Calls: 


sfc ak ok 2K 2k ae a 2K a He ae eC a ae aK AK a A A aK oR a a A a a HE ae a A IC ee 9K AK HK AK He A CR A A KK KK CK HK ACK HK 


void 
getConstantsforForerunner(double sigma) 


{ 


/* constant for steering function used in simulation */ 


k = 1.0 / sigma; 

ass kK: 

b=3 *k* k; 

cak? kk: 
} 
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Module Name: runForerunner function 
Purpose: simulate the robot with arbirtary sigma 
Parameters: q, L 

Returns: q 

Called by: main 

Calls: getKappaQ), circQ, composite() 


ake 9 2 2 fe 2 ee ee 2K 2 2K ee 2K 9 AK 2 He 2K 2 2K EK AC EH He A 2 ER HK HK HK HK A KE EK KK KE / 


CONFIGURATION 
runForerunner((;CONFIGURATION g, CONFIGURATION L) 


CONFIGURATION deltaQ; 
double dkappa; 

double theta; 

double deltaS = 0.1; 


dkappa = q.Kappa + getKappa(q, L) * deltaS; 
theta = deltaS * dkappa; 

deltaQ = circ(deltaS, theta); 

q = composite(q, deltaQ); 

q.Kappa = dkappa; 


return q; 
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Module Name: getKappa function 
Purpose: compute kappa for virtual robot 
Parameters: q, L 

Returns: kappa 

Called by: main 

Calls: 


ake ok ok 2k 2 oe 2K 2k 2k ae ak oe 2 ae 2 eo 2 ee AK A HK KH a A A A IK A A A AK KK KK KKK HK BK HK / 


double 
getKappa(CONFIGURATION q, CONFIGURATION L) 
{ 


double deltaD; 


deltaD = -(q.Posit.X - L.Posit.X) * sin(L.Theta) + 
(q.Posit. Y - L.Posit.Y) * cos(L.Theta); 


return -(a * q.Kappa + b * (q.Theta - L.Theta) + c * deltaD); 
} 
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Module Name: checklfcollide function 
Purpose: test the robot’s colliding of wall 
Parameters: q 

Returns: 

Called by: main 


Calls: caculateAngle() 
2 A A A AR A A A A A A He He A He A AK A A A AR HK IK RK A EK AE KR ACHE KE / 


int 
checklfcollide( CONFIGURATION gq) 
{ 
int 1, J; 
double angle = 0.0; 
CONFIGURATION qCorner[4]; 


qCorner[0] = composite(q, robotCorner[0]); 
qCorner[1] = composite(q, robotCorner[1]); 
qCorner[2] = composite(q, robotCorner[2]); 
qCorner[3] = composite(q, robotCorner[3]); 


for (i = 0; 1 < 4; i++) { 
angle = calculateAngle(qCorner[i], 3, 0); 
for G = 0; } < 3; j++) { 
angle = angle + calculateAngle(qCorner{i}, j, } + 1); 
} 


if (angle > -PI) return 1; 


} 


return 0; 
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Module Name: calculateAngle function 
Purpose: calculate the view angle of robot 
Parameters: q, 1, J 

Returns: angle 

Called by: checklfcollide() 

Calis: 


2k 2 2k 2 fe 2k 2k 2k ae ae 2 ae a a a ae a a A a a A A KK I AK aK KK KK IK A A He ee AK AK KK KK KK KK EE EK / 


double 
calculateAngle( CONFIGURATION 4q, int 1, int J) 


{ 
return norm(atan2(corner[j].Y - q.Posit.Y, corner[j].X - q.Posit.X) - 


atan2(corner[i].Y - q.Posit.Y, corner[i].X - q.Posit.X)); 
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Module Name: checkIfQparallelQE function 
Purpose: test the robot is parallel to the reference line 
Parameters: ql, q2 

Returns: 

Called by: main 

Calls: 


2 He 2 2 He 2 9 2 A eR AR He He AR Ke He HHH HK He ACH He He A A HH HK HH A AE A HH A HER / 
int 
checkIfQparallelQE(CONFIGURATION ql, CONFIGURATION q2) 


{ 
double dtheta; 


dtheta = q1.Theta - q2.Theta; 
if (dtheta >= -0.1 && dtheta <= 0.1) return 1; 


return 0; 


} 
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Module Name: checkIfQequalsQE function 
Purpose: test the robot gets to the ge 
Parameters: q 

Returns: 

Called by: main 

Calls: 


ake 3 2K KE CK OR AK aK a AK a a A AK aK a HE a A A AK KK A AK KK He a IK A A A KK A KK KR KK EK | 


int 

checkIfQequalsQE(CONFIGURATION q) 

{ 
if (q.Posit.Y >= -0.01 && q.Posit.Y <= 0.01) return 1; 
return QO; 

j 
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D. C LIBRARY FUNCTIONS 
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Module Name: computeQstar function 

Purpose: calculate the local coordinate between two configurations 
Parameters: ql, q2 

Returns: qstar 

Called by: 

Calls: 


2 2 fe 26 2K ae 2k 2 oe 2k 2 2h 2 2 2 HK AK AK He A A A A I A AK I RK BK KK KKK KAKA ER / 


CONFIGURATION 
computeQstar(CONFIGURATION ql, CONFIGURATION q2) 
{ 


CONFIGURATION gstar; 


qstar.Posit.X = (q1.Posit.X - q2.Posit.X) * 
cos(q2.Theta) + 
(q1.Posit.Y - q2.Posit.Y) * 
sin(q2.Theta); 

qstar.Posit. Y = (q1.Posit. Y - q2.Posit.Y) * 
cos(q2.Theta) - 
(q1.Posit.X - q2.Posit.X) * 
sin(q2. Theta); 

qstar.Theta =ql1.Theta - q2.Theta; 


return qstar; 


} 
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Module Name: composite function 
Purpose: compose two configurations 
Parameters: q1, q2 

Returns: 

Called by: 

Calls: 


af ae of a 2 2k 2 ae ak 2 2 2 ae he Ae oe 2 ae he he oe 2 2 2 he a A a AR eK A ae a I AR a A A KK HK KK KE AC / 


CONFIGURATION 
composite(CONFIGURATION qi, CONFIGURATION q2) 


CONFIGURATION q; 

q.Posit.X = q1.Posit.X + q2.Posit.X * cos(q1.Theta) - 
q2.Posit. Y * sin(q1.Theta); 

q.Posit. Y = q1.Posit.Y + q2.Posit.X * sin(q1.Theta) + 
q2.Posit.Y * cos(q1.Theta); 

q.Theta = q1.Theta + q2.Theta; 


return q; 
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Module Name: circ function 
Purpose: get configuration 
Parameters: deltaS, theta 
Returns: q 

Called by: 

Calls: 


2 2 26 2k 2 2&3 fe fe 3 ae 2 2k ee 2 2 he he 2h 2 2 he 2K 2 HH Ae A He A AE A He A AK A AE HB AH A A A A RE KE / 


CONFIGURATION 
circ(double deltaS, double theta) 


CONFIGURATION gq; 

q.Posit.X = (1.0 - theta * theta / 6) * deltaS; 

q.Posit.Y = (1.0 / 2.0 - theta * theta / 24.0)* theta * deltaS; 
q. Theta = theta; 


return q; 
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Module Name: norm function 
Purpose: normalize orientation 
Parameters: angle 

Returns: 

Called by: 

Calis: 


2 He 3 2c 2k ok 2 2 2 ae fe 2K ae eK ae a AC ag He ae a eK A HK a A He A CH A He AR He AK HK A A A KE AH A  / 


double 
norm(double angle) 
{ 
while ((angle >= PI) Il (angle < -PI)) 
{ 
if (angle >= PI) 
angle -= 2 * PI; 
else 
angle += 2 * PI; 
} 
return angle; 


} 
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